#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <cmath>

// 全局变量：存储上一次的位置信息
double last_x = 0.0, last_y = 0.0;
bool is_first_msg = true;  // 标志位，判断是否是第一条消息
double total_distance = 0.0;  // 累计移动距离

// 回调函数：计算移动距离
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    double current_x = msg->pose.pose.position.x;
    double current_y = msg->pose.pose.position.y;

    if (!is_first_msg) {
        double dx = current_x - last_x;
        double dy = current_y - last_y;
        double distance = std::sqrt(dx * dx + dy * dy);

        // 添加阈值判断，忽略微小的位移
        const double distance_threshold = 0.005;  // 阈值（单位：米）
        if (distance > distance_threshold) {
            total_distance += distance;
        }

        ROS_INFO("Current Distance: %.2f, Total Distance: %.2f", distance, total_distance);
    } else {
        is_first_msg = false;
    }

    last_x = current_x;
    last_y = current_y;
}

int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "odometry_distance_node");

    // 创建节点句柄
    ros::NodeHandle nh;

    // 订阅里程计话题
    ros::Subscriber odom_sub = nh.subscribe("/odom", 10, odomCallback);

    // ROS循环
    ros::spin();

    return 0;
}